// Kevin Kemper

#include <fca_controllers/controller.h>

extern void initialize_motor_torque_controller(ControllerInput *input, ControllerOutput *output, ControllerState *state, 
	ControllerData *data)
{
	output->motor_torque = 0.;
	output->time = 0.0;
	MTR_TRQ_CONTROLLER_STATE(state)->time = 0.0;
}


extern void update_motor_torque_controller(ControllerInput *input, ControllerOutput *output, ControllerState *state, 
	ControllerData *data)
{
	
	float trq = MTR_TRQ_CONTROLLER_DATA(data)->mtr_trq;
	
	MTR_TRQ_CONTROLLER_STATE(state)->time += 0.001;
	output->time = MTR_TRQ_CONTROLLER_STATE(state)->time;
	
	output->motor_torque = trq*(1.0/0.18)*(-1.0/17.25);
	
	
}


extern void takedown_motor_torque_controller(ControllerInput *input, ControllerOutput *output, ControllerState *state, 
	ControllerData *data)
{
	output->motor_torque = 0.;
	output->time = 0.0;
	MTR_TRQ_CONTROLLER_STATE(state)->time = 0.0;
}
